#ifndef WHEEL_PREINTEGRATION_H
#define WHEEL_PREINTEGRATION_H
#include <iostream>
#include <vector>

#include "math_utils.h"
#include "state.h"
#include "se2.h"

struct WheelOptions {
   WheelOptions() {}
    double sigma_x = 1.0e-5; 
    double sigma_y = 1.0e-5;
    double sigma_t = 1.0e-5;    
};

class WheelPreIntegration {
public:
   WheelPreIntegration(WheelOptions options = WheelOptions());
   void Integrate(const SE2& last_odometry, const SE2& current_odometry);
   void Predict(State& state);
   void Clear();
   SO2 GetSO2();
   Eigen::Vector2d GetTranslation();
   Eigen::Matrix<double, 3, 3> GetCov();
public:
   WheelOptions options_;

   SO2 dR_;
   Eigen::Vector2d dp_ = Eigen::Vector2d::Zero();  
   Eigen::Matrix<double, 3, 3> cov_ = Eigen::Matrix<double, 3, 3>::Zero();     
};

#endif 
